#include "replanning.h"
#include <chrono>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/path.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/empty.hpp>
#include <std_msgs/msg/float32.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

bool   Constants::debug;
int    Constants::dir_discrete_num;
double Constants::dist_from_obs2target;

double Constants::cellSize;
int    Constants::iterations;

double Constants::bloating;
double Constants::car_length_;
double Constants::veh_W;
double Constants::veh_Lf;
double Constants::veh_Lr;
double Constants::expand_step;
double Constants::move_delta;
double Constants::r;

std::vector< double > Constants::dx;
std::vector< double > Constants::dy;
std::vector< double > Constants::dt;

double Constants::penaltyTurning;
double Constants::penaltyReversing;
double Constants::penaltySteerChange;
double Constants::dubinsShotDistance;
double Constants::dubinsStepSize;
double Constants::move_step;
int    Constants::headings           = 72;
double Constants::deltaHeadingRad    = 2 * M_PI / Constants::headings;
double Constants::deltaHeadingNegRad = 2 * M_PI - Constants::deltaHeadingRad;
double Constants::tieBreaker         = 0.01;
double Constants::minRoadWidth       = 2;

class ReplanNode : public rclcpp::Node {
public:
    ReplanNode() : rclcpp::Node( "Replan" ) {
        initialParamters();
        createParamScene();
        createPublishAndSubscriber();
    }

    void initialParamters() {
        // param for replanning
        this->declare_parameter( "debug", true );
        this->get_parameter< bool >( "debug", Constants::debug );
        RCLCPP_INFO( get_logger(), "debug: %d", Constants::debug );

        this->declare_parameter( "dir_discrete_num", 2 );
        this->get_parameter< int >( "dir_discrete_num", Constants::dir_discrete_num );
        RCLCPP_INFO( get_logger(), "dir_discrete_num: %d", Constants::dir_discrete_num );

        this->declare_parameter( "reverse_dist", 1.0 );
        this->get_parameter< double >( "reverse_dist", reverse_dist );
        RCLCPP_INFO( get_logger(), "reverse_dist: %f", reverse_dist );

        this->declare_parameter( "dist_from_obs2target", 6.0 );
        this->get_parameter< double >( "dist_from_obs2target", Constants::dist_from_obs2target );
        RCLCPP_INFO( get_logger(), "dist_from_obs2target: %f", Constants::dist_from_obs2target );

        this->declare_parameter( "iterations", 300000 );
        this->get_parameter< int >( "iterations", Constants::iterations );
        RCLCPP_INFO( get_logger(), "iterations: %d", Constants::iterations );

        this->declare_parameter( "car_length_", 1.587 );
        this->get_parameter< double >( "car_length_", Constants::car_length_ );
        RCLCPP_INFO( get_logger(), "car_length_: %f", Constants::car_length_ );

        this->declare_parameter( "veh_W", 1.0 );
        this->get_parameter< double >( "veh_W", Constants::veh_W );
        RCLCPP_INFO( get_logger(), "veh_W: %f", Constants::veh_W );

        this->declare_parameter( "veh_Lf", 1.85 );
        this->get_parameter< double >( "veh_Lf", Constants::veh_Lf );
        RCLCPP_INFO( get_logger(), "veh_Lf: %f", Constants::veh_Lf );

        this->declare_parameter( "veh_Lr", 0.2 );
        this->get_parameter< double >( "veh_Lr", Constants::veh_Lr );
        RCLCPP_INFO( get_logger(), "veh_Lr: %f", Constants::veh_Lr );

        this->declare_parameter( "bloating", 0.1 );
        this->get_parameter< double >( "bloating", Constants::bloating );
        RCLCPP_INFO( get_logger(), "bloating: %f", Constants::bloating );

        this->declare_parameter( "expand_step", 0.45 );
        this->get_parameter< double >( "expand_step", Constants::expand_step );
        RCLCPP_INFO( get_logger(), "expand_step: %f", Constants::expand_step );

        this->declare_parameter( "move_delta", 10.0 );
        this->get_parameter< double >( "move_delta", Constants::move_delta );
        RCLCPP_INFO( get_logger(), "move_delta: %f", Constants::move_delta );

        this->declare_parameter( "penaltyTurning", 1.5 );
        this->get_parameter< double >( "penaltyTurning", Constants::penaltyTurning );
        RCLCPP_INFO( get_logger(), "penaltyTurning: %f", Constants::penaltyTurning );

        this->declare_parameter( "penaltyReversing", 10.0 );
        this->get_parameter< double >( "penaltyReversing", Constants::penaltyReversing );
        RCLCPP_INFO( get_logger(), "penaltyReversing: %f", Constants::penaltyReversing );

        this->declare_parameter( "penaltySteerChange", 2.0 );
        this->get_parameter< double >( "penaltySteerChange", Constants::penaltySteerChange );
        RCLCPP_INFO( get_logger(), "penaltySteerChange: %f", Constants::penaltySteerChange );

        this->declare_parameter( "dubinsShotDistance", 2.0 );
        this->get_parameter< double >( "dubinsShotDistance", Constants::dubinsShotDistance );
        RCLCPP_INFO( get_logger(), "dubinsShotDistance: %f", Constants::dubinsShotDistance );

        this->declare_parameter( "dubinsStepSize", 0.05 );
        this->get_parameter< double >( "dubinsStepSize", Constants::dubinsStepSize );
        RCLCPP_INFO( get_logger(), "dubinsStepSize: %f", Constants::dubinsStepSize );

        this->declare_parameter( "move_step", 0.05 );
        this->get_parameter< double >( "move_step", Constants::move_step );
        RCLCPP_INFO( get_logger(), "move_step: %f", Constants::move_step );

        Constants::cellSize = 0.05;
        Constants::veh_W += 2 * Constants::bloating;
        Constants::veh_Lf += Constants::bloating;
        Constants::veh_Lr += Constants::bloating;
        Constants::expand_step /= Constants::cellSize;  // 相对于栅格地图的步长
        Constants::move_delta *= M_PI / 180.0;
        Constants::r = Constants::expand_step / Constants::move_delta;

        Constants::dx.push_back( Constants::expand_step );
        Constants::dy.push_back( 0 );
        Constants::dt.push_back( 0 );
        for ( int i = 0; i < Constants::dir_discrete_num + 1; i++ ) {
            double move_delta_i = Constants::move_delta * ( i + 1 ) / ( Constants::dir_discrete_num + 1 );
            Constants::dx.push_back( Constants::expand_step * sin( move_delta_i ) / move_delta_i );
            Constants::dy.push_back( Constants::expand_step * ( 1 - cos( move_delta_i ) ) / move_delta_i );
            Constants::dt.push_back( move_delta_i );
        }
        for ( int i = 0; i < Constants::dir_discrete_num + 1; i++ ) {
            double move_delta_i = Constants::move_delta * ( i + 1 ) / ( Constants::dir_discrete_num + 1 );
            Constants::dx.push_back( Constants::expand_step * sin( move_delta_i ) / move_delta_i );
            Constants::dy.push_back( -Constants::expand_step * ( 1 - cos( move_delta_i ) ) / move_delta_i );
            Constants::dt.push_back( -move_delta_i );
        }

        Constants::dubinsStepSize /= Constants::cellSize;
        Constants::move_step /= Constants::cellSize;

        replanner_ = std::make_shared< Replanning >();
    }

    void createParamScene() {
        // Eigen::Vector3d obs1( 20.5, -7.5, -1.57 );
        // vehicle_obstacles.emplace_back( obs1 );
        // start       = Eigen::Vector3d( 19.664, -2.327, 1.597 );
        // goal        = Eigen::Vector3d( 19.500, -12.350, 1.571 );
        // motion_type = -1;

        Eigen::Vector3d obs1( 13.000, -0.200, 0.000 );
        vehicle_obstacles.emplace_back( obs1 );
        start       = Eigen::Vector3d( 9.265, 0.388, 3.142 );
        goal        = Eigen::Vector3d( 19.087, -0.351, 2.238 );
        motion_type = -1;
    }

    void createPublishAndSubscriber() {
        gridmap_subscriber_ = this->create_subscription< nav_msgs::msg::OccupancyGrid >(
            "/map", rclcpp::QoS( 1 ), std::bind( &ReplanNode::GridmapCallback, this, std::placeholders::_1 ) );
        obstacles_subscriber_ = this->create_subscription< geometry_msgs::msg::PoseArray >(
            "obstacles_pose", rclcpp::QoS( 1 ), std::bind( &ReplanNode::ObsCallback, this, std::placeholders::_1 ) );
        ref_vel_subscriber = this->create_subscription< std_msgs::msg::Float32 >(
            "ref_vel", rclcpp::QoS( 1 ), std::bind( &ReplanNode::VelCallback, this, std::placeholders::_1 ) );
        last_ref_path_subscriber_ = this->create_subscription< nav_msgs::msg::Path >(
            "trajectory_path", rclcpp::QoS( 1 ), std::bind( &ReplanNode::TrajCallback, this, std::placeholders::_1 ) );
        pose_subscriber_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped >(
            "pcl_pose", rclcpp::QoS( 1 ), std::bind( &ReplanNode::PoseCallback, this, std::placeholders::_1 ) );
        sim_current_pose_subscriber_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped >(
            "sim_pcl_pose", rclcpp::QoS( 1 ), std::bind( &ReplanNode::SimPoseCallback, this, std::placeholders::_1 ) );

        active_replan_subscriber_ = this->create_subscription< std_msgs::msg::Empty >(
            "active_replan", rclcpp::QoS( 1 ),
            std::bind( &ReplanNode::ActiveReplanCallback, this, std::placeholders::_1 ) );

        obs_visual_publisher_ = this->create_publisher< visualization_msgs::msg::MarkerArray >( "obstacles_shape", 1 );
        replanning_path_visual_publisher_ =
            this->create_publisher< nav_msgs::msg::Path >( "replanning_path_visual", 1 );
        new_ref_traj_publisher_       = this->create_publisher< nav_msgs::msg::Path >( "replanning_traj", 1 );
        replan_feedback_publisher_    = this->create_publisher< std_msgs::msg::Bool >( "replan_feedback", 1 );
        first_replan_begin_publisher_ = this->create_publisher< std_msgs::msg::Bool >( "first_replan_begin", 1 );

        RCLCPP_INFO( rclcpp::get_logger( "" ), "Create publish and subscriber" );

        timer_ =
            this->create_wall_timer( std::chrono::milliseconds( 100 ), std::bind( &ReplanNode::replan_loop, this ) );
    }

    void GridmapCallback( const nav_msgs::msg::OccupancyGrid::SharedPtr msg ) {
        if ( !safe_gridmap.check() ) {
            safe_gridmap.set( msg );
            RCLCPP_INFO( rclcpp::get_logger( "" ), "[TIC] Subscribe image map" );
            Constants::cellSize = msg->info.resolution;
            replanner_->calcGridmapData(
                safe_gridmap.get() );  // get后safe_gridmap.check()会返回false; 如果地图一直发那么将会一直接收
            isExitMap = true;
        } else
            return;
    }

    void ObsCallback( const geometry_msgs::msg::PoseArray &msg ) {
        // 需要区分普通障碍物和车辆障碍物
        // 比较新来的障碍物是否是新的障碍物
        std::vector< Eigen::Vector3d > new_veh_obstacles;
        for ( const geometry_msgs::msg::Pose &p : msg.poses ) {
            bool bIsNewObs = true;
            for ( const auto &veh_obs : vehicle_obstacles ) {
                if ( fabs( p.position.x - veh_obs.x() ) < 0.05 && fabs( p.position.y - veh_obs.y() < 0.05 )
                     && fabs( p.position.z - veh_obs.z() ) < 5 * M_PI / 180 ) {
                    RCLCPP_INFO( get_logger(), "[TIC] The same obstacles!" );
                    bIsNewObs = false;
                }
            }
            if ( bIsNewObs ) {
                new_veh_obstacles.emplace_back( Eigen::Vector3d( p.position.x, p.position.y, p.position.z ) );
            }
        }

        if ( !new_veh_obstacles.empty() ) {
            vehicle_obstacles.insert( vehicle_obstacles.end(), new_veh_obstacles.begin(), new_veh_obstacles.end() );
            replanner_->updateGridmap( normal_obstacles, vehicle_obstacles );
            active_replan = true;
            RCLCPP_INFO( get_logger(), "[TIC] Subscribe obstacles pose msg" );
        }
    }

    void VelCallback( const std_msgs::msg::Float32 msg ) {
        ref_vel = static_cast< double >( msg.data );
    }

    void TrajCallback( const nav_msgs::msg::Path &msg ) {
        if ( !msg.poses.empty() ) {
            RCLCPP_INFO( get_logger(), "[TIC] Subscribe last reference traj msg" );

            history_last_ref_path = msg;
            vector< traj > last_ref_traj;
            NavPathtoTraj( msg, last_ref_traj );
            replanner_->setRefTraj( last_ref_traj );
        }
    }

    void NavPathtoTraj( const nav_msgs::msg::Path &msg, vector< traj > &last_ref_traj ) {
        for ( const auto &pose : msg.poses ) {
            traj tmp_t;
            tmp_t.x   = pose.pose.position.x;
            tmp_t.y   = pose.pose.position.y;
            tmp_t.phi = tf2::getYaw( pose.pose.orientation );
            tmp_t.vel = ref_vel;
            last_ref_traj.push_back( tmp_t );
        }
    }

    void PoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped &msg ) {
        current_pos.x   = msg.pose.pose.position.x;
        current_pos.y   = msg.pose.pose.position.y;
        current_pos.phi = tf2::getYaw( msg.pose.pose.orientation );
        RCLCPP_INFO( rclcpp::get_logger( "" ), "current_pos: [%.3f,%.3f,%.3f]", current_pos.x, current_pos.y,
                     current_pos.phi );
    }

    void SimPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped &msg ) {
        current_pos.x   = msg.pose.pose.position.x;
        current_pos.y   = msg.pose.pose.position.y;
        current_pos.phi = tf2::getYaw( msg.pose.pose.orientation );
        RCLCPP_INFO( rclcpp::get_logger( "" ), "current_pos: [%.3f,%.3f,%.3f]", current_pos.x, current_pos.y,
                     current_pos.phi );
    }

    void ActiveReplanCallback( const std_msgs::msg::Empty ) {
        active_replan = true;
    }

    void replan_loop() {
        if ( isExitMap ) {
            auto time_start = std::chrono::high_resolution_clock::now();

            replanner_->updateGridmap( normal_obstacles, vehicle_obstacles );
            visualizeObs();
            RCLCPP_INFO( rclcpp::get_logger( "" ), "Begin Replanning!!!" );
            replanner_->findStartAndTargetState( start, goal, motion_type );

            if ( replanner_->runReplanning() ) {

                RCLCPP_INFO( rclcpp::get_logger( "" ), "Replanning is Over!!!" );

                nav_msgs::msg::Path replanning_path;
                replanner_->getReplanningPath( replanning_path );

                replanning_path_visual_publisher_->publish( replanning_path );
                RCLCPP_INFO( rclcpp::get_logger( "" ), "visulize replanning path" );

                auto                            time_end = std::chrono::high_resolution_clock::now();
                std::chrono::duration< double > diff     = ( time_end - time_start ) * 1000;
                RCLCPP_INFO( rclcpp::get_logger( "" ), "replan cost time: %.5f ms]", diff.count() );
            }
            isExitMap = false;
            vehicle_obstacles.clear();
            normal_obstacles.clear();
        } else {
            RCLCPP_INFO( rclcpp::get_logger( "" ), "IDLE" );
        }
    }

    void visualizeObs() {
        visualization_msgs::msg::MarkerArray obs_marker_array;
        for ( int i = 0; i < normal_obstacles.size(); i++ ) {
            visualization_msgs::msg::Marker obs_marker;
            setObsShape( normal_obstacles[ i ], obs_marker, i, "normal" );
            obs_marker_array.markers.push_back( obs_marker );
        }
        for ( int i = 0; i < vehicle_obstacles.size(); i++ ) {
            visualization_msgs::msg::Marker obs_marker;
            setObsShape( vehicle_obstacles[ i ], obs_marker, i, "vehicle" );
            obs_marker_array.markers.push_back( obs_marker );
        }
        obs_visual_publisher_->publish( obs_marker_array );
    }

    void setObsShape( const Eigen::Vector3d &obs, visualization_msgs::msg::Marker &obs_marker, int i, string type ) {
        obs_marker.header.frame_id = "map";
        obs_marker.header.stamp    = this->now();
        obs_marker.ns              = type + "_obs_" + to_string( i );
        obs_marker.id              = 0;
        if ( type == "normal" ) {
            obs_marker.type            = visualization_msgs::msg::Marker::CYLINDER;
            obs_marker.action          = visualization_msgs::msg::Marker::ADD;
            obs_marker.pose.position.x = obs[ 0 ];
            obs_marker.pose.position.y = obs[ 1 ];
            obs_marker.pose.position.z = 0;

            obs_marker.scale.x = 2 * obs[ 2 ];  // Diameter along x-axis
            obs_marker.scale.y = 2 * obs[ 2 ];  // Diameter along y-axis
            obs_marker.scale.z = 0.01;          // Height
        } else if ( type == "vehicle" ) {
            obs_marker.type            = visualization_msgs::msg::Marker::CUBE;
            obs_marker.action          = visualization_msgs::msg::Marker::ADD;
            obs_marker.pose.position.x = obs.x() + Constants::car_length_ / 2 * cos( obs.z() );
            obs_marker.pose.position.y = obs.y() + Constants::car_length_ / 2 * sin( obs.z() );
            obs_marker.pose.position.z = 0;

            tf2::Quaternion quat;
            quat.setRPY( 0, 0, obs.z() );  // 使用欧拉角设置绕 z 轴旋转45度
            obs_marker.pose.orientation.x = quat.x();
            obs_marker.pose.orientation.y = quat.y();
            obs_marker.pose.orientation.z = quat.z();
            obs_marker.pose.orientation.w = quat.w();

            obs_marker.scale.x = Constants::veh_Lf + Constants::veh_Lr;
            obs_marker.scale.y = Constants::veh_W;
            obs_marker.scale.z = 0.01;
        }

        obs_marker.color.r = 0.0f;
        obs_marker.color.g = 0.0f;
        obs_marker.color.b = 0.5f;
        obs_marker.color.a = 0.5;
    }

private:
    std::shared_ptr< Replanning >                   replanner_;
    safe_class< nav_msgs::msg::OccupancyGrid::Ptr > safe_gridmap;
    vector< Eigen::Vector3d >                       normal_obstacles;   // (x, y, r)
    vector< Eigen::Vector3d >                       vehicle_obstacles;  // (x, y, yaw)
    nav_msgs::msg::Path                             history_last_ref_path;
    double                                          ref_vel = 0;
    traj                                            current_pos;
    double                                          reverse_dist = 0;

    Eigen::Vector3d start;
    Eigen::Vector3d goal;
    int             motion_type;

    bool active_replan = false;
    bool isExitMap     = false;

    rclcpp::Subscription< nav_msgs::msg::OccupancyGrid >::SharedPtr                  gridmap_subscriber_;
    rclcpp::Subscription< geometry_msgs::msg::PoseArray >::SharedPtr                 obstacles_subscriber_;
    rclcpp::Subscription< nav_msgs::msg::Path >::SharedPtr                           last_ref_path_subscriber_;
    rclcpp::Subscription< std_msgs::msg::Float32 >::SharedPtr                        ref_vel_subscriber;
    rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr pose_subscriber_;
    rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr sim_current_pose_subscriber_;

    rclcpp::Subscription< std_msgs::msg::Empty >::SharedPtr active_replan_subscriber_;

    std::shared_ptr< rclcpp::TimerBase > timer_;

    rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr obs_visual_publisher_;
    rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr                  replanning_path_visual_publisher_;
    rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr                  new_ref_traj_publisher_;
    rclcpp::Publisher< std_msgs::msg::Bool >::SharedPtr                  replan_feedback_publisher_;
    rclcpp::Publisher< std_msgs::msg::Bool >::SharedPtr                  first_replan_begin_publisher_;
};

int main( int argc, char **argv ) {
    rclcpp::init( argc, argv );
    auto node = std::make_shared< ReplanNode >();
    rclcpp::spin( node );
    return 0;
}
